// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Pose.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.Runtime.Serialization;

    /// <summary>
    /// Pose class
    /// </summary>
    [DataContract]
    public struct Pose
    {
        /// <summary>
        /// Identity pose object
        /// </summary>
        public static readonly Pose Identity = new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1));

        /// <summary>
        /// The pose position field
        /// </summary>
        [DataMember(Name = "Position")]
        private Vector3 position;

        /// <summary>
        /// The pose orientation field
        /// </summary>
        [DataMember(Name = "Orientation")]
        private Quaternion orientation;

        /// <summary>
        /// Initializes a new instance of the Pose struct.
        /// </summary>
        /// <param name="position">Position vector</param>
        public Pose(Vector3 position)
        {
            this.position = new Vector3(position);
            this.orientation = new Quaternion(0.0, 0.0, 0.0, 1.0);
        }

        /// <summary>
        /// Initializes a new instance of the Pose struct.
        /// </summary>
        /// <param name="position">Position vector</param>
        /// <param name="orientation">Orientation quaternion</param>
        public Pose(Vector3 position, Quaternion orientation)
        {
            this.position = new Vector3(position);
            this.orientation = new Quaternion(orientation);
        }

        /// <summary>
        /// Initializes a new instance of the Pose struct.
        /// </summary>
        /// <param name="pose">Pose to be copied</param>
        public Pose(Pose pose)
        {
            this.position = new Vector3(pose.position);
            this.orientation = new Quaternion(pose.orientation);
        }

        /// <summary>
        /// Initializes a new instance of the Pose struct.
        /// </summary>
        /// <param name="position">Position vector. </param>
        /// <param name="zyxEulerAngles">Orientation expressed as Z-Y-X Euler angles. </param>
        public Pose(Vector3 position, Vector3 zyxEulerAngles)
        {
            Matrix4 ht = HomogenousTransform.CreateFromRotationZ(zyxEulerAngles.Z) *
                HomogenousTransform.CreateFromRotationY(zyxEulerAngles.Y) *
                HomogenousTransform.CreateFromRotationX(zyxEulerAngles.X);

            this.orientation = new Quaternion(ht);

            this.position = position;
        }

        /// <summary>
        /// Initializes a new instance of the Pose struct.
        /// </summary>
        /// <param name="homogenousTransform">A homogenous transform. </param>
        public Pose(Matrix4 homogenousTransform)
        {
            this.orientation = new Quaternion(homogenousTransform);

            this.position = new Vector3(
                homogenousTransform[0, 3],
                homogenousTransform[1, 3],
                homogenousTransform[2, 3]);
        }

        /// <summary>
        /// Gets or sets the pose position
        /// </summary>
        public Vector3 Position
        {
            get
            {
                return this.position;
            }

            set
            {
                this.position = value;
            }
        }

        /// <summary>
        /// Gets or sets the pose orientation
        /// </summary>
        public Quaternion Orientation
        {
            get
            {
                return this.orientation;
            }

            set
            {
                this.orientation = value;
            }
        }

        /// <summary>
        /// The equals operator
        /// </summary>
        /// <param name="leftSide">Pose on the left side</param>
        /// <param name="rightSide">Pose on the right side</param>
        /// <returns>Are the Pose values equal</returns>
        public static bool operator ==(Pose leftSide, Pose rightSide)
        {
            // Return true if the fields match:
            return leftSide.orientation == rightSide.orientation && leftSide.position == rightSide.position;
        }

        /// <summary>
        /// The not equals operator
        /// </summary>
        /// <param name="leftSide">Pose on the left side</param>
        /// <param name="rightSide">Pose on the right side</param>
        /// <returns>The not equal result</returns>
        public static bool operator !=(Pose leftSide, Pose rightSide)
        {
            return !(leftSide == rightSide);
        }

        /// <summary>
        /// Retrieves a string representation of the Pose.
        /// </summary>
        /// <returns>String that represents the Pose.</returns>
        public override string ToString()
        {
            return string.Format("Position=({0}), Orientation=({1})", this.position, this.orientation);
        }

        /// <summary>
        /// Checks for equality of two Poses.
        /// </summary>
        /// <param name="obj">Pose to be compared</param>
        /// <returns>Comparison result</returns>
        public override bool Equals(object obj)
        {
            if (!(obj is Pose))
            {
                return false;
            }

            Pose poseObj = (Pose)obj;

            return this.position == poseObj.position && this.orientation == poseObj.orientation;
        }

        /// <summary>
        /// Returns HashCode of object.
        /// </summary>
        /// <returns>Hashcode result</returns>
        public override int GetHashCode()
        {
            return base.GetHashCode();
        }

        /// <summary>
        /// Gets the Z-Y-X Euler angles for the orientation component of the pose.
        /// See <see href="http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles"/>
        /// </summary>
        /// <returns> The Z-Y-X Euler angles for the orientation component of the pose. </returns>
        public Vector3 GetZYXEulerAngles()
        {
           Quaternion q = this.orientation;

           return new Vector3(
               Math.Atan2(2 * (q.W * q.X + q.Y * q.Z), 1 - 2 * (q.X * q.X + q.Y * q.Y)),
               Math.Asin(2 * (q.W * q.Y - q.Z * q.X)),
               Math.Atan2(2 * (q.W * q.Z + q.X * q.Y), 1 - 2 * (q.Y * q.Y + q.Z * q.Z)));
        }
    }
}
